//
// Created by lyx on 2020/8/22.
//

#ifndef RANSAC_PLANES_PLANE_INFO_H
#define RANSAC_PLANES_PLANE_INFO_H
//require Eigen;require Utils
#include <Utils.h>
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>
using namespace std;
using namespace Eigen;
using Vec3f=lyxutils::Vector3<float>;

struct PlaneInfo{
    Vec3f center;
    Vec3f xAxis,yAxis,zAxis;//unit orthogonal vectors representing each axis
    Vec3f sigma;//sigma along each axis,determine the scale along each axis
    //the coordinates of point p1 and p2(in local reference system),
    //where p1 has the maximum projection along x axis,y axis and z axis;
    //and p2 has the minimum projection along x axis,y axis and z axis
    Vec3f maxXYZ,minXYZ;
    int ptCount;//number of point in this plane
    void setViewPoint(const Vec3f &pt);
    //get four vertices of this plane
    //p-p_ave=[xAXis,yAXis,zAXis]*p',where p is coordinates in
    // world coordinate system and p' is in local system
    // resultant pts contains four points p1,p2,p3,p4(four conner of a rectangle in clock-wise)
    void getVertices(vector<Vec3f> &pts)const;
    void parseLine(string line);
    friend ostream& operator << (ostream &output,const PlaneInfo &info);
};
//fit a plane using points in pts,the indices are ptIndice
template<typename PointT,typename allocator=std::allocator<PointT> >
void planeFit(const std::vector<PointT,allocator> &pts, const vector<int> &ptIndice,
              Vec3f &center, Vec3f &sigma, Vec3f &xAxis, Vec3f &yAxis, Vec3f &zAxis){
    Matrix3d cov_mat=Matrix3d::Zero();
    //get averages
    double xm=0,ym=0,zm=0;
    for(int j=0;j<ptIndice.size();++j){
        auto p=pts.at(ptIndice[j]);
        xm+=p.x;
        ym+=p.y;
        zm+=p.z;
    }
    xm/=ptIndice.size();
    ym/=ptIndice.size();
    zm/=ptIndice.size();
    for(int j=0;j<ptIndice.size();++j){
        auto p=pts.at(ptIndice[j]);
        double dx=p.x-xm,dy=p.y-ym,dz=p.z-zm;
        cov_mat(0,0)+=dx*dx;
        cov_mat(0,1)+=dx*dy;
        cov_mat(0,2)+=dx*dz;
        cov_mat(1,1)+=dy*dy;
        cov_mat(1,2)+=dy*dz;
        cov_mat(2,2)+=dz*dz;
    }
    cov_mat(1,0)=cov_mat(0,1);
    cov_mat(2,0)=cov_mat(0,2);
    cov_mat(2,1)=cov_mat(1,2);
    EigenSolver<Matrix3d> es(cov_mat);
    Eigen::Matrix3d D = es.pseudoEigenvalueMatrix();
    Eigen::Matrix3d V = es.pseudoEigenvectors();
    int minIdx=0,maxIdx=0,midIdx=0;
    double minD=D(0,0),maxD=D(0,0);
    for(int j=0;j<3;++j){
        if(D(j,j)<minD){
            minD=D(j,j);
            minIdx=j;
        }
        if(D(j,j)>maxD){
            maxD=D(j,j);
            maxIdx=j;
        }
    }
    if(minIdx==maxIdx){//three equal eigen values
        minIdx=0;
        midIdx=1;
        maxIdx=2;
    }
    else{
        midIdx=3-minIdx-maxIdx;
    }
    center.x=xm;
    center.y=ym;
    center.z=zm;
    sigma={float(D(maxIdx,maxIdx)),float(D(midIdx,midIdx)),float(D(minIdx,minIdx))};
    xAxis={float(V(0,maxIdx)),float(V(1,maxIdx)),float(V(2,maxIdx))};
    yAxis={float(V(0,midIdx)),float(V(1,midIdx)),float(V(2,midIdx))};
    zAxis={float(V(0,minIdx)),float(V(1,minIdx)),float(V(2,minIdx))};
}
#endif //RANSAC_PLANES_PLANE_INFO_H
